/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

/*
*/

#include <iostream>
#include <math.h>
#include "gimi.h"
#include "gimiprotocoltypes.h"
#include "FSRSim/Protocol.h"
#include "owndebug.h"
#include "ownutils.h"



std::string targetName;

void initGIMI(gimi::GIMI *gim)
{
	
	int r;
	r=gim->connectToHubEx("aut-183.hut.fi",50002,"");
	if (r==GIMI_OK)
	{
		printf("Connected\n");
		
		gim->addProvidedService(GIMI_PROTOCOL_FSRSIM_OBJECT_REPOSITION,GIMI_IGNOREMINORTYPE,"repos cmds");
		
	} else if (r==GIMI_INVALIDNAME){
		//wxString s=_T("Couldn't connect GIMI: Name ");
		//s << name << "already in use.";
		//wxMessageBox(s);
		printf("Couldn't connect GIMI: Name already in use.\n");
	} else {
		//wxMessageBox("Couldn't connect GIMI: GIMI_ERROR.");
		printf("Couldn't connect GIMI: GIMI_ERROR.\n");
	}

  
}

void sendCmd(gimi::GIMI *gim, TFSRSimObjectRepositionCmd *repos)
{
	int size;
	const char *data=repos->createData(size);
	printf("sending position %f %f %f %f\n",repos->x,repos->y,repos->z,repos->heading);
	int r=gim->send(targetName,data,size,GIMI_PROTOCOL_FSRSIM_OBJECT_REPOSITION,GIMI_IGNOREMINORTYPE);
	if (r==GIMI_OK)
	{
		printf("sent cmd\n");
	} else if (r==GIMI_ERROR) {
		printf("couldnt send, GIMI_ERROR\n");
	} else if (r==GIMI_NOCONNECTION) {
		printf("couldnt send, GIMI_NOCONNECTION\n");
	}
}

int main(void)
{
	gimi::GIMI gim;
	TFSRSimObjectRepositionCmd repos;

	bool go=true;

	debugInit();
	debugSetGlobalDebugLvl(1);
  debugAddMode(DEBUG_MODE_IMMEDIATE_FLUSH);

	initGIMI(&gim);

	char s[100];
	printf("Give target robot name\n");
	scanf("%s",s);
	targetName=s;
  /*
	while (go)
	{
		float x,y,z;
		printf("give position x, y, z and heading (deg) in form x y z heading\n");
		scanf("%f %f %f %f",&(repos.x),&(repos.y),&(repos.z),&(repos.heading));
		repos.heading=repos.heading*(M_PI/180.0);
		sendCmd(&gim,&repos);

	}*/

  ownSleep_ms(2000);

  for (int i=0;i<50;i++)
  {
    repos.x=1.0*i/10;
    repos.y=0;
    repos.z=0;
    repos.heading=i/100.0;
    sendCmd(&gim,&repos);
  }

}
